﻿using FIRADriverLibrary;
using GeometryCSharp;
using System.IO;
using System.Drawing;
using System;

namespace AGHFiraPlayer
{
    public abstract class BezierCurveDriver
    {
        public static double robotDiameter = 0.075;
 //       public static double robotDiameter = 0.175;
        public static Point2D checkPoint;
        public static double fast = 0.2;
        public static double medium = 0.1;
        public static double slow = 0.06;
        public static double bezzereFactor = 0.5; //was 0,03
        public static StreamWriter sw = new StreamWriter("C:\\file.txt");
   //     public static StreamWriter sw2 = new StreamWriter("C:\\Points.txt");
        public static RectangleF pitchArea = Predicator.pitchArea;

        public static void driveWithVelocityVector(FIRARobotState robot, double x, double y, double velocity, Vector2D velocityVector)
        {
            Point2D point = new Point2D(x, y);
           // point = point - velocityVector*(robotDiameter/2);

            double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
            double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;

            checkPoint = calculateBezierCheckPoint(robot, point.x, point.y, velocity, velocityVector);
            if (checkPoint.x == -1 || checkPoint.y == -1)
            {
                robot.leftSpeed = 0;
                robot.rightSpeed = 0;
                return;
            }
            linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;	//might have chaged
            deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
            if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
            {
                BezierCurveDriver.calculateContorolCircle(robot, checkPoint, velocity);
            }
            else
            {
                BezierCurveDriver.calculateContorolCircle(robot, point, velocity);
            }
         //   robot.leftSpeed = linearVel + deltaVel;
         //   robot.rightSpeed = linearVel - deltaVel;

        }
        public static BezierCurve calculateBezzierCurve(FIRARobotState robot, Point2D destiny, double velocity, Vector2D velocityVector)
        {
            Point2D a = new Point2D(robot.x, robot.y);
            double distance = a.GetDistance(destiny);
            Vector2D robotVector = new Vector2D(robot.angle);
            //problem gdy piłka goni naszego robota
            double robotSpeed = (robot.currentLeftSpeed + robot.currentRightSpeed) / 2.0;
            if (robotSpeed < 0)
            {
                robotVector.Rotate(Math.PI);
            }
            double bx = robot.x + robotVector.X * (0.001 + robotSpeed * distance);
            double by = robot.y + robotVector.Y * (0.001 + robotSpeed * distance);
            double cx = destiny.x - velocityVector.X * velocity * distance;
            double cy = destiny.y - velocityVector.Y * velocity * distance;

            Point2D b = new Point2D(bx, by);
            Point2D c = new Point2D(cx, cy);

            Vector2D dest = new Vector2D(c, destiny);
            BezierCurve bc = new BezierCurve(a, b, c, destiny);
            if (bc.isInPitch(pitchArea.Height, pitchArea.Width, robotDiameter))
            {
                return bc;
            }
            return null;
        }

        private static Point2D calculateBezierCheckPoint(FIRARobotState robot, double x, double y, double velocity, Vector2D velocityVector)
        {

            BezierCurve bc = calculateBezzierCurve(robot,new Point2D(x,y),velocity,velocityVector);
            if (bc != null)
            {
                    return bc.evaluate(0.3);
            }
            else
                return new Point2D(-1, -1);
        }

     
        public static void calculateContorolCircle(FIRARobotState robot,Point2D dest,double velocity)
        {
  
            double factor= 1; //jesli dodatni to znacyz ze jedziemy w  lewo - zcwiekszamy predkosc na prawym kole
            Point2D u1 = new Point2D(robot.x, robot.y);
            Vector2D robotVector = new Vector2D(robot.angle);
            if (velocity < 0)
            {
                robotVector.Rotate(Math.PI);
                factor = -1;
            }
            Point2D u2 = u1 + robotVector;
            Point2D s = BezierCurve.calculateCircleMiddlePoint(u1, u2, dest);

     
            if (s == null)
            {
                robot.leftSpeed = velocity;
                robot.rightSpeed = velocity;
                return;
            }
            Vector2D sv = new Vector2D(u1, dest);

            if (Math.Sin(robotVector.AngleBetween(sv)) > 0)

            {

                factor = factor * -1;
            }
            else
            {

                factor = factor * 1;
            }
            double distance = s.GetDistance(u1);
            double rightWheelDistance = distance + factor * robotDiameter ;
            double leftWheelDistance = distance - factor * robotDiameter;
            double ratio = rightWheelDistance/leftWheelDistance;

            double leftWheelVelocity = 0;
            double rightWheelVelocity = 0;
            if (ratio < 0)
            {
                leftWheelVelocity = (2 * velocity) / (1 - ratio);
                rightWheelVelocity = 2 * velocity - leftWheelVelocity;
            }
            else
            {
                leftWheelVelocity = (2 * velocity) / (1 + ratio);
                rightWheelVelocity = 2 * velocity - leftWheelVelocity;
            }

           

 
            robot.leftSpeed = leftWheelVelocity;
            robot.rightSpeed = rightWheelVelocity;
            if (leftWheelVelocity > 2 || rightWheelVelocity > 2)
            {
                sw.WriteLine();
                sw.WriteLine("VELOCITY " + velocity);
                sw.WriteLine();
                sw.WriteLine("LEFT " + leftWheelVelocity);
                sw.WriteLine("RIGHT " + rightWheelVelocity);
                sw.WriteLine();
                sw.WriteLine("distanceL " + leftWheelDistance);
                sw.WriteLine("distanceR " + rightWheelDistance);
                sw.WriteLine();
            }
        }
    /*    public static void Main()
        {
            caculateContorolCircle(null, new Point2D(1, 1), 2);

        }
     */


    }
}
